home *** CD-ROM | disk | FTP | other *** search
/ GFX Sensations 1 / Graphic Sensations - Volume 1.iso / tools / amiga / 3d_tools / irit40s.lha / Irit / misc_lib / genmat.c < prev    next >
Encoding:
C/C++ Source or Header  |  1993-12-30  |  9.7 KB  |  288 lines
  1. /*****************************************************************************
  2. * General matrix manipulation routines.                         *
  3. *                                         *
  4. * Written by:  Gershon Elber                   Ver 1.0,    June 1988    *
  5. *****************************************************************************/
  6.  
  7. #include <math.h>
  8. #include "irit_sm.h"
  9. #include "genmat.h"
  10.  
  11. /*****************************************************************************
  12. * Routine to generate a 4*4 unit matrix:                                     *
  13. *****************************************************************************/
  14. void MatGenUnitMat(MatrixType Mat)
  15. {
  16.     int i, j;
  17.  
  18.     for (i = 0; i < 4; i++)
  19.     for (j = 0; j < 4; j++)
  20.         if (i == j)
  21.         Mat[i][j] = 1.0;
  22.         else
  23.         Mat[i][j] = 0.0;
  24. }
  25.  
  26. /*****************************************************************************
  27. * Routine to generate a 4*4 matrix to translate in Tx, Ty, Tz amounts.       *
  28. *****************************************************************************/
  29. void MatGenMatTrans(RealType Tx, RealType Ty, RealType Tz, MatrixType Mat)
  30. {
  31.     MatGenUnitMat(Mat);                             /* Make it unit matrix. */
  32.     Mat[3][0] = Tx;
  33.     Mat[3][1] = Ty;
  34.     Mat[3][2] = Tz;
  35. }
  36.  
  37. /*****************************************************************************
  38. * Routine to generate a 4*4 matrix to Scale x, y, z in Sx, Sy, Sz amounts.   *
  39. *****************************************************************************/
  40. void MatGenMatScale(RealType Sx, RealType Sy, RealType Sz, MatrixType Mat)
  41. {
  42.     MatGenUnitMat(Mat);                              /* Make it unit matrix. */
  43.     Mat[0][0] = Sx;
  44.     Mat[1][1] = Sy;
  45.     Mat[2][2] = Sz;
  46. }
  47.  
  48. /*****************************************************************************
  49. * Routine to generate a 4*4 matrix to Rotate around X with angle of Teta.    *
  50. * Note Teta must be given in radians.                                        *
  51. *****************************************************************************/
  52. void MatGenMatRotX1(RealType Teta, MatrixType Mat)
  53. {
  54.     RealType CTeta, STeta;
  55.  
  56.     CTeta = cos(Teta);
  57.     STeta = sin(Teta);
  58.     MatGenMatRotX(CTeta, STeta, Mat);
  59. }
  60.  
  61. /*****************************************************************************
  62. * Routine to generate a 4*4 matrix to Rotate around X with angle of Teta.    *
  63. *****************************************************************************/
  64. void MatGenMatRotX(RealType CosTeta, RealType SinTeta, MatrixType Mat)
  65. {
  66.     MatGenUnitMat(Mat);                              /* Make it unit matrix. */
  67.     Mat[1][1] = CosTeta;
  68.     Mat[1][2] = SinTeta;
  69.     Mat[2][1] = -SinTeta;
  70.     Mat[2][2] = CosTeta;
  71. }
  72.  
  73. /*****************************************************************************
  74. * Routine to generate a 4*4 matrix to Rotate around Y with angle of Teta.    *
  75. * Note Teta must be given in radians.                                        *
  76. *****************************************************************************/
  77. void MatGenMatRotY1(RealType Teta, MatrixType Mat)
  78. {
  79.     RealType CTeta, STeta;
  80.  
  81.     CTeta = cos(Teta);
  82.     STeta = sin(Teta);
  83.     MatGenMatRotY(CTeta, STeta, Mat);
  84. }
  85.  
  86. /*****************************************************************************
  87. * Routine to generate a 4*4 matrix to Rotate around Y with angle of Teta.    *
  88. *****************************************************************************/
  89. void MatGenMatRotY(RealType CosTeta, RealType SinTeta, MatrixType Mat)
  90. {
  91.     MatGenUnitMat(Mat);                              /* Make it unit matrix. */
  92.     Mat[0][0] = CosTeta;
  93.     Mat[0][2] = -SinTeta;
  94.     Mat[2][0] = SinTeta;
  95.     Mat[2][2] = CosTeta;
  96. }
  97.  
  98. /*****************************************************************************
  99. * Routine to generate a 4*4 matrix to Rotate around Z with angle of Teta.    *
  100. * Note Teta must be given in radians.                                        *
  101. *****************************************************************************/
  102. void MatGenMatRotZ1(RealType Teta, MatrixType Mat)
  103. {
  104.     RealType CTeta, STeta;
  105.  
  106.     CTeta = cos(Teta);
  107.     STeta = sin(Teta);
  108.     MatGenMatRotZ(CTeta, STeta, Mat);
  109. }
  110.  
  111. /*****************************************************************************
  112. * Routine to generate a 4*4 matrix to Rotate around Z with angle of Teta.    *
  113. *****************************************************************************/
  114. void MatGenMatRotZ(RealType CosTeta, RealType SinTeta, MatrixType Mat)
  115. {
  116.     MatGenUnitMat(Mat);                              /* Make it unit matrix. */
  117.     Mat[0][0] = CosTeta;
  118.     Mat[0][1] = SinTeta;
  119.     Mat[1][0] = -SinTeta;
  120.     Mat[1][1] = CosTeta;
  121. }
  122.  
  123. /*****************************************************************************
  124. * Routine to multiply 2 4by4 matrices:                                       *
  125. * Note MatRes might be one of Mat1 or Mat2 - it is only updated in the end!  *
  126. *****************************************************************************/
  127. void MatMultTwo4by4(MatrixType MatRes, MatrixType Mat1, MatrixType Mat2)
  128. {
  129.     int i, j, k;
  130.     MatrixType MatResTemp;
  131.  
  132.     for (i = 0; i < 4; i++)
  133.     for (j = 0; j < 4; j++) {
  134.        MatResTemp[i][j] = 0;
  135.        for (k = 0; k < 4; k++)
  136.            MatResTemp[i][j] += Mat1[i][k] * Mat2[k][j];
  137.     }
  138.     for (i = 0; i < 4; i++)
  139.     for (j = 0; j < 4; j++)
  140.         MatRes[i][j] = MatResTemp[i][j];
  141. }
  142.  
  143. /*****************************************************************************
  144. * Routine to add 2 4by4 matrices:                         *
  145. * Note MatRes might be one of Mat1 or Mat2.                     *
  146. *****************************************************************************/
  147. void MatAddTwo4by4(MatrixType MatRes, MatrixType Mat1, MatrixType Mat2)
  148. {
  149.     int i, j;
  150.  
  151.     for (i = 0; i < 4; i++)
  152.         for (j = 0; j < 4; j++)
  153.             MatRes[i][j] = Mat1[i][j] + Mat2[i][j];
  154. }
  155.  
  156. /*****************************************************************************
  157. * Routine to subtract 2 4by4 matrices:                         *
  158. * Note MatRes might be one of Mat1 or Mat2.                     *
  159. *****************************************************************************/
  160. void MatSubTwo4by4(MatrixType MatRes, MatrixType Mat1, MatrixType Mat2)
  161. {
  162.     int i, j;
  163.  
  164.     for (i = 0; i < 4; i++)
  165.         for (j = 0; j < 4; j++)
  166.         MatRes[i][j] = Mat1[i][j] - Mat2[i][j];
  167. }
  168.  
  169. /*****************************************************************************
  170. * Routine to scale a 4by4 matrix by constant:                     *
  171. * Note MatRes might be Mat.                             *
  172. *****************************************************************************/
  173. void MatScale4by4(MatrixType MatRes, MatrixType Mat, RealType *Scale)
  174. {
  175.     int i, j;
  176.  
  177.     for (i = 0; i < 4; i++)
  178.         for (j = 0; j < 4; j++)
  179.         MatRes[i][j] = Mat[i][j] * (*Scale);
  180. }
  181.  
  182. /*****************************************************************************
  183. * Routine to multiply Vector by 4by4 matrix:                                 *
  184. * The Vector has only 3 components (X, Y, Z) and it is assumed that W = 1    *
  185. * Note VRes might be Vec as it is only updated in the end.                   *
  186. *****************************************************************************/
  187. void MatMultVecby4by4(VectorType VRes, VectorType Vec, MatrixType Mat)
  188. {
  189.     int i, j;
  190.     RealType
  191.     CalcW = Mat[3][3];
  192.     VectorType VTemp;
  193.  
  194.     for (i = 0; i < 3; i++) {
  195.     VTemp[i] = Mat[3][i];         /* Initiate it with the weight factor. */
  196.     for (j = 0; j < 3; j++)
  197.         VTemp[i] += Vec[j] * Mat[j][i];
  198.     }
  199.  
  200.     for (i = 0; i < 3; i++)
  201.     CalcW += Vec[i] * Mat[i][3];
  202.     if (CalcW == 0)
  203.     CalcW = 1 / INFINITY;
  204.  
  205.     for (i = 0; i < 3; i++)
  206.     VRes[i] = VTemp[i] / CalcW;
  207. }
  208.  
  209. /*****************************************************************************
  210. * Routine to multiply Vector by 4by4 matrix:                                 *
  211. * The Vector has 4 components (X, Y, Z, W).                     *
  212. * Note VRes might be Vec as it is only updated in the end.                   *
  213. *****************************************************************************/
  214. void MatMultWVecby4by4(RealType VRes[4], RealType Vec[4], MatrixType Mat)
  215. {
  216.     int i, j;
  217.     RealType VTemp[4];
  218.  
  219.     for (i = 0; i < 4; i++) {
  220.         VTemp[i] = 0.0;
  221.         for (j = 0; j < 4; j++)
  222.         VTemp[i] += Vec[j] * Mat[j][i];
  223.     }
  224.  
  225.     for (i = 0; i < 4; i++)
  226.     VRes[i] = VTemp[i];
  227. }
  228.  
  229. /*****************************************************************************
  230. * Procedure to calculate the INVERSE of a given matrix M which is not        *
  231. * modified. The matrix is assumed to be 4 by 4 (transformation matrix).      *
  232. * Return TRUE if inverted matrix (InvM) do exists.                 *
  233. *****************************************************************************/
  234. int MatInverseMatrix(MatrixType M, MatrixType InvM)
  235. {
  236.     MatrixType A;
  237.     int i, j, k;
  238.     RealType V;
  239.  
  240.     MAT_COPY(A, M);            /* Prepare temporary copy of M in A. */
  241.     MatGenUnitMat(InvM);                 /* Make it unit matrix. */
  242.  
  243.     for (i = 0; i < 4; i++) {
  244.     V = A[i][i];                      /* Find the new pivot. */
  245.     k = i;
  246.     for (j = i + 1; j < 4; j++)
  247.         if (ABS(A[j][i]) > ABS(V)) {
  248.             /* Find maximum on col i, row i+1..n */
  249.             V = A[j][i];
  250.             k = j;
  251.         }
  252.     j = k;
  253.  
  254.     if (i != j)
  255.         for (k = 0; k < 4; k++) {
  256.         SWAP(RealType, A[i][k], A[j][k]);
  257.         SWAP(RealType, InvM[i][k], InvM[j][k]);
  258.             }
  259.  
  260.     for (j = i + 1; j < 4; j++) {     /* Eliminate col i from row i+1..n. */
  261.             V = A[j][i] / A[i][i];
  262.         for (k = 0; k < 4; k++) {
  263.                 A[j][k]    -= V * A[i][k];
  264.                 InvM[j][k] -= V * InvM[i][k];
  265.         }
  266.     }
  267.     }
  268.  
  269.     for (i = 3; i >= 0; i--) {                   /* Back Substitution. */
  270.     if (A[i][i] == 0)
  271.         return FALSE;                       /* Error. */
  272.  
  273.     for (j = 0; j < i; j++) {     /* Eliminate col i from row 1..i-1. */
  274.             V = A[j][i] / A[i][i];
  275.         for (k = 0; k < 4; k++) {
  276.                 /* A[j][k] -= V * A[i][k]; */
  277.                 InvM[j][k] -= V * InvM[i][k];
  278.         }
  279.     }
  280.     }
  281.  
  282.     for (i = 0; i < 4; i++)            /* Normalize the inverse Matrix. */
  283.     for (j = 0; j < 4; j++)
  284.             InvM[i][j] /= A[i][i];
  285.  
  286.     return TRUE;
  287. }
  288.